/**********************************************************************
 Copyright (c) 2020-2023, Unitree Robotics.Co.Ltd. All rights reserved.
***********************************************************************/

#include "example_walk.h"

using namespace UNITREE_LEGGED_SDK;

std::string Custom::forward = "forward";
std::string Custom::left = "left";
std::string Custom::right = "right";
std::string Custom::backward = "backward";

void Custom::UDPRecv()
{
    udp.Recv();
}

void Custom::UDPSend()
{
    udp.Send();
}

void Custom::RobotControl()
{
    motiontime += 2;
    udp.GetRecv(state);
    printf("%d   %f\n", motiontime, state.imu.quaternion[2]);

    cmd.mode = 0; // 0:idle, default stand      1:forced stand     2:walk continuously
    cmd.gaitType = 0;
    cmd.speedLevel = 0;
    cmd.footRaiseHeight = 0;
    cmd.bodyHeight = 0;
    cmd.euler[0] = 0;
    cmd.euler[1] = 0;
    cmd.euler[2] = 0;
    cmd.velocity[0] = 0.0f;
    cmd.velocity[1] = 0.0f;
    cmd.yawSpeed = 0.0f;
    cmd.reserve = 0;

    if (motiontime > 0 && motiontime < 1000)
    {
        cmd.mode = 1;
        cmd.euler[0] = -0.3;
    }
    if (motiontime > 1000 && motiontime < 2000)
    {
        cmd.mode = 1;
        cmd.euler[0] = 0.3;
    }
    if (motiontime > 2000 && motiontime < 3000)
    {
        cmd.mode = 1;
        cmd.euler[1] = -0.2;
    }
    if (motiontime > 3000 && motiontime < 4000)
    {
        cmd.mode = 1;
        cmd.euler[1] = 0.2;
    }
    if (motiontime > 4000 && motiontime < 5000)
    {
        cmd.mode = 1;
        cmd.euler[2] = -0.2;
    }
    if (motiontime > 5000 && motiontime < 6000)
    {
        cmd.mode = 1;
        cmd.euler[2] = 0.2;
    }
    if (motiontime > 6000 && motiontime < 7000)
    {
        cmd.mode = 1;
        cmd.bodyHeight = -0.2;
    }
    if (motiontime > 7000 && motiontime < 8000)
    {
        cmd.mode = 1;
        cmd.bodyHeight = 0.1;
    }
    if (motiontime > 8000 && motiontime < 9000)
    {
        cmd.mode = 1;
        cmd.bodyHeight = 0.0;
    }
    if (motiontime > 9000 && motiontime < 11000)
    {
        cmd.mode = 5;
    }
    if (motiontime > 11000 && motiontime < 13000)
    {
        cmd.mode = 6;
    }
    if (motiontime > 13000 && motiontime < 14000)
    {
        cmd.mode = 0;
    }
    if (motiontime > 14000 && motiontime < 18000)
    {
        cmd.mode = 2;
        cmd.gaitType = 2;
        cmd.velocity[0] = 0.4f; // -1  ~ +1
        cmd.yawSpeed = 2;
        cmd.footRaiseHeight = 0.1;
        // printf("walk\n");
    }
    if (motiontime > 18000 && motiontime < 20000)
    {
        cmd.mode = 0;
        cmd.velocity[0] = 0;
    }
    if (motiontime > 20000 && motiontime < 24000)
    {
        cmd.mode = 2;
        cmd.gaitType = 1;
        cmd.velocity[0] = 0.2f; // -1  ~ +1
        cmd.bodyHeight = 0.1;
        // printf("walk\n");
    }
    if (motiontime > 2400)
    {
        cmd.mode = 1;
    }

    udp.SetSend(cmd);
}

void Custom::RobotWalk()
{
    motiontime += 2;

    udp.GetRecv(state);
    printf("%d   %f\n", motiontime, state.imu.quaternion[2]);

    cmd.mode = 2; // 0:idle, default stand      1:forced stand     2:walk continuously
    cmd.gaitType = 2;
    cmd.speedLevel = 0;
    cmd.footRaiseHeight = 0.1;
    cmd.bodyHeight = 0;
    cmd.euler[0] = 0;
    cmd.euler[1] = 0;
    cmd.euler[2] = 0;
    cmd.velocity[0] = 0.4f; // -1  ~ +1
    cmd.velocity[1] = 0.0f;
    cmd.reserve = 0;
    cmd.yawSpeed = 2;

    if (motiontime > 24000)
    {
        cmd.mode = 1;
        udp.SetSend(cmd);
        return;
    }

    udp.SetSend(cmd);
}

void Custom::dogStartTest(int type)
{
    std::cout << "Communication level is set to HIGH-level." << std::endl
              << "WARNING: Make sure the robot is standing on the ground." << std::endl
              << "Press Enter to continue..." << std::endl;
    std::cin.ignore();

    Custom custom(HIGHLEVEL);
    
    LoopFunc loop_udpSend("udp_send", custom.dt, 3, boost::bind(&Custom::UDPSend, &custom));
    LoopFunc loop_udpRecv("udp_recv", custom.dt, 3, boost::bind(&Custom::UDPRecv, &custom));
    LoopFunc loop_control("control_loop", custom.dt, boost::bind(&Custom::RobotControl, &custom));
    LoopFunc loop_walk_control("walk_control_loop", custom.dt, boost::bind(&Custom::RobotWalk, &custom));

    loop_udpSend.start();
    loop_udpRecv.start();

    switch (type) {
        case ALL_TEST:
            loop_control.start();
            break;
        case WALK_TEST:
            loop_walk_control.start();
            break;
        default:
            break;
    }

    while (1)
    {
        sleep(10);
    };

}

void Custom::RobotForward() {
    
}

void Custom::RobotLeft() {

}

void Custom::RobotRight() {

}

void Custom::RobotBackward() {

}